/**
 * @FilePath: \ros2\src\agv_control\chassis_controller\src\chassis_controller.cpp
 * @Date: 2025-03-07 19:20:47
 * @Description:  
 * @Author: haibo
 * @Version: 1.0.0
 * @LastEditors: haibo haibo.yang@lgmgim.cn
 * @LastEditTime: 2025-03-20 19:42:51
 * @Copyright (c) 2025 by 临工智能信息科技有限公司, All Rights Reserved. 
**/

#include "chassis_controller/chassis_controller.hpp"
#include "chassis_controller/odometry_compensation.h"

#include <limits>
#include <memory>
#include <string>
#include <vector>
#include <unordered_set>

#include "controller_interface/helpers.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"

#include "agv_msgs/msg/driver_state.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/logging.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> 
#include <shared_parameters/shared_params_interface.hpp>

namespace
{  // utility

// TODO(destogl): remove this when merged upstream
// Changed services history QoS to keep all so we don't lose any client service calls
static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = {
  RMW_QOS_POLICY_HISTORY_KEEP_ALL,
  1,  // message queue depth
  RMW_QOS_POLICY_RELIABILITY_RELIABLE,
  RMW_QOS_POLICY_DURABILITY_VOLATILE,
  RMW_QOS_DEADLINE_DEFAULT,
  RMW_QOS_LIFESPAN_DEFAULT,
  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
  false};

using ControllerTwistReferenceMsg = chassis_controller::ChassisController::ControllerTwistReferenceMsg;

// called from RT control loop
void reset_controller_reference_msg(
  std::shared_ptr<ControllerTwistReferenceMsg> & msg, 
  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
{
  msg->header.stamp = node->now();
  msg->twist.linear.x = std::numeric_limits<double>::quiet_NaN();
  msg->twist.linear.y = std::numeric_limits<double>::quiet_NaN();
  msg->twist.linear.z = std::numeric_limits<double>::quiet_NaN();
  msg->twist.angular.x = std::numeric_limits<double>::quiet_NaN();
  msg->twist.angular.y = std::numeric_limits<double>::quiet_NaN();
  msg->twist.angular.z = std::numeric_limits<double>::quiet_NaN();
}

}  // namespace

namespace chassis_controller
{
using namespace std::chrono_literals;
using controller_interface::interface_configuration_type;
using controller_interface::InterfaceConfiguration;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
using hardware_interface::HW_IF_EFFORT;
using lifecycle_msgs::msg::State;

ChassisController::ChassisController() : controller_interface::ControllerInterface(),
                                        cmd_vel_timeout_(0.5),
                                        base_frame_id_("base_footprint"),
                                        odom_frame_id_("odom"),
                                        enable_odom_tf_(true),
                                        angle_tolerance_(0.2),
                                        hardware_interface_count_(0),
                                        drivers_state_ready_(false),
                                        publish_period_(rclcpp::Duration::from_seconds(0.1))
{ 
}

controller_interface::CallbackReturn ChassisController::on_init()
{
  DEBUG_OUT("Loading controller...");
  
  if (!SharedParamsInterface::initialize()) {
    DEBUG_ERROR_OUT("Failed to initialize shared memory interface");
    return controller_interface::CallbackReturn::ERROR;
  }

  try {
    auto params = SharedParamsInterface::getNodeParams("shared_parameters");
    if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
      DEBUG_ERROR_OUT("Error get shared parameters");
      return controller_interface::CallbackReturn::ERROR;
    } 

    params = SharedParamsInterface::getNodeParams("base");
    for (const auto& [name, value] : params) {
      size_t pos = name.find("twist_controller/");
      if (pos != std::string::npos) {
        pos += std::string("twist_controller/").length();
        size_t end = name.find(" ", pos);
        std::string field_name = name.substr(pos, end - pos);
        DEBUG_STREAM_OUT("Parameter:" << field_name << ", value:" << value.toString());
        switch(value.type) {
          case SharedParamsInterface::ParamValue::Type::INT:
              get_node()->declare_parameter(field_name, value.getValue<int>());
              break;
          case SharedParamsInterface::ParamValue::Type::DOUBLE:
              get_node()->declare_parameter(field_name, value.getValue<double>());
              break;
          case SharedParamsInterface::ParamValue::Type::STRING:
              get_node()->declare_parameter(field_name, value.getValue<std::string>());
              break;
          case SharedParamsInterface::ParamValue::Type::BOOL:
              get_node()->declare_parameter(field_name, value.getValue<bool>());
              break;
          default:
              DEBUG_WARN_OUT("Unsupported parameter type for " << field_name);
              break;
        }
      } else {
        continue;
      }    
    }
  } catch (const std::exception& e) {
    DEBUG_ERROR_OUT("Error reading parameters: " << e.what());
    return controller_interface::CallbackReturn::ERROR;
  }

  return controller_interface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn ChassisController::on_configure(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  DEBUG_STREAM_OUT("on_configure");
  if (!config()) {
    DEBUG_WARN_OUT("Parameter config error");
    return controller_interface::CallbackReturn::FAILURE;
  }
  
  // topics QoS
  auto subscribers_qos = rclcpp::SystemDefaultsQoS();
  subscribers_qos.keep_last(1);
  subscribers_qos.best_effort();

  // Reference Subscriber
  double ref_timeout;
  get_node()->get_parameter_or("reference_timeout", ref_timeout, 0.1);
  ref_timeout_ = rclcpp::Duration::from_seconds(ref_timeout);
  ref_subscriber_twist_ = get_node()->create_subscription<ControllerTwistReferenceMsg>(
    "/cmd_vel", subscribers_qos,
    std::bind(&ChassisController::reference_callback, this, std::placeholders::_1));
  
  sub_driver_info_= get_node()->create_subscription<agv_msgs::msg::DriversInfo>(
    "/driversInfo", subscribers_qos,
    std::bind(&ChassisController::drivers_info_callback, this, std::placeholders::_1));
  
  std::shared_ptr<ControllerTwistReferenceMsg> msg = std::make_shared<ControllerTwistReferenceMsg>();
  reset_controller_reference_msg(msg, get_node());
  input_ref_.writeFromNonRT(msg);
  
  try {
    // Odom state publisher
    odom_publisher_ = get_node()->create_publisher<ControllerStateMsgOdom>(
        "/odom", rclcpp::SystemDefaultsQoS());
    rt_odom_state_publisher_ = std::make_shared<
        realtime_tools::RealtimePublisher<ControllerStateMsgOdom>>(odom_publisher_);
  } catch (const std::exception & e) {
    fprintf(stderr, 
        "Exception thrown during publisher creation at configure stage with message : %s \n",
        e.what());
    return controller_interface::CallbackReturn::ERROR;
  }

  auto & odometry_message = rt_odom_state_publisher_->msg_;
  odometry_message.header.frame_id = odom_frame_id_;
  odometry_message.child_frame_id = base_frame_id_;

  try {
    // Tf State publisher
    tf_odom_publisher_ = get_node()->create_publisher<ControllerStateMsgTf>(
        "tf", rclcpp::SystemDefaultsQoS());
    rt_tf_odom_state_publisher_ = std::make_shared<
        realtime_tools::RealtimePublisher<ControllerStateMsgTf>>(tf_odom_publisher_);
  } catch (const std::exception & e) {
    fprintf(stderr, 
        "Exception thrown during publisher creation at configure stage with message : %s \n",
        e.what());
    return controller_interface::CallbackReturn::ERROR;
  }

  auto & odometry_transform_message = rt_tf_odom_state_publisher_->msg_;
  odometry_transform_message.transforms.resize(1);
  odometry_transform_message.transforms.front().header.frame_id = odom_frame_id_;
  odometry_transform_message.transforms.front().child_frame_id = base_frame_id_;

  try {
    streer_publisher_ = get_node()->create_publisher<agv_msgs::msg::SteerDirFeedBack>(
        "/steer_state", rclcpp::SystemDefaultsQoS());
    rt_streer_publisher_ = std::make_shared<
        realtime_tools::RealtimePublisher<agv_msgs::msg::SteerDirFeedBack>>(streer_publisher_);
  } catch (const std::exception & e) {
    fprintf(stderr, 
            "Exception thrown during publisher creation at configure stage with message : %s \n",
            e.what());
    return controller_interface::CallbackReturn::ERROR;
  }

  auto & streer_message = rt_streer_publisher_->msg_;
  streer_message.id = 0;
  streer_message.steering = 0;
  streer_message.steer_err = 0;

  DEBUG_OUT("configure successful");
  return controller_interface::CallbackReturn::SUCCESS;
}

void ChassisController::reference_callback(const std::shared_ptr<ControllerTwistReferenceMsg> msg)
{
  DEBUG_STREAM_OUT("reference_callback");
  // if no timestamp provided use current time for command timestamp
  if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) {
    RCLCPP_WARN(get_node()->get_logger(),
        "Timestamp in header is missing, using current time as command timestamp.");
        msg->header.stamp = get_node()->now();
  }
  const auto age_of_last_command = get_node()->now() - msg->header.stamp;

  if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) {
    input_ref_.writeFromNonRT(msg);
  } else { 
    RCLCPP_ERROR(get_node()->get_logger(),
        "Received message has timestamp %.10f older for %.10f which is more then allowed timeout "
        "(%.4f).", rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(),
        ref_timeout_.seconds());
  }
}

controller_interface::InterfaceConfiguration ChassisController::command_interface_configuration() const
{
  controller_interface::InterfaceConfiguration command_interfaces_config;
  command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  DEBUG_STREAM_OUT("command_interface_configuration");

  for (int i = 0; i < pivot_num_; ++i) {  
    for (int j=0; j<2; ++j) {
      const auto& joint_param = pivot_param_[i].joint_param[j];
      if (joint_param.name.empty()) continue;
      DEBUG_STREAM_OUT("Add cmd inf:" << joint_param.name);
      command_interfaces_config.names.push_back(joint_param.name + "/" + hardware_interface::HW_IF_VELOCITY);
      command_interfaces_config.names.push_back(joint_param.name + "/" + hardware_interface::HW_IF_POSITION);
    }
  }  
  
  return command_interfaces_config;       
}

controller_interface::InterfaceConfiguration ChassisController::state_interface_configuration() const
{
  controller_interface::InterfaceConfiguration state_interfaces_config;
  state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  DEBUG_STREAM_OUT("state_interface_configuration");
  for (int i = 0; i < pivot_num_; ++i) {
    // 外部编码器接口
    if (!pivot_param_[i].external_encoder_name.empty() && pivot_param_[i].use_external_encoder) {
      DEBUG_STREAM_OUT("state inf joint encoder [" << i << "]:" << pivot_param_[i].external_encoder_name);
      state_interfaces_config.names.push_back(
        pivot_param_[i].external_encoder_name + "/" + hardware_interface::HW_IF_POSITION);
    }

    // 关节状态接口
    for (int j = 0; j < 2; ++j) {
      const auto& joint_param = pivot_param_[i].joint_param[j];
      if (joint_param.name.empty()) continue;
      DEBUG_STREAM_OUT("Add state inf:" << joint_param.name);
      state_interfaces_config.names.push_back(joint_param.name + "/" + hardware_interface::HW_IF_VELOCITY);
      state_interfaces_config.names.push_back(joint_param.name + "/" + hardware_interface::HW_IF_POSITION);
      state_interfaces_config.names.push_back(joint_param.name + "/" + hardware_interface::HW_IF_EFFORT);
    }
  }
  return state_interfaces_config;
}

controller_interface::CallbackReturn ChassisController::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  DEBUG_STREAM_OUT("on_activate");
  // Set default value in command
  reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node());

  // get hardware_interface handler
  for (int i = 0; i < pivot_num_; ++i) {   
    //获取编码器joint_handler
    if (!pivot_param_[i].external_encoder_name.empty() && pivot_param_[i].use_external_encoder ) {
      auto result = configure_side(ENCODER, pivot_param_[i].external_encoder_name, pivot_param_[i].encoder_handle);

      // used_joint_handle_vector_.push_back(joint_handle_vector_[k]);
      if (result == controller_interface::CallbackReturn::ERROR) {
          DEBUG_ERROR_OUT("Get " << pivot_param_[i].external_encoder_name << "handle joint false!");
          return controller_interface::CallbackReturn::FAILURE;
      }
    }

    //获取电机joint_handler
    for (int j=0; j<2; ++j) {
      if (!pivot_param_[i].joint_param[j].name.empty()) {
        DEBUG_STREAM_OUT(
            "Adding wheel with joint name: " << pivot_param_[i].joint_param[j].name);

        if (pivot_param_[i].joint_param[j].drive_mode == TRACTION
            || pivot_param_[i].joint_param[j].drive_mode == STEER) {
          joint_type drive_mode = static_cast<joint_type>(pivot_param_[i].joint_param[j].drive_mode);
          std::string name = pivot_param_[i].joint_param[j].name;
          auto result = configure_side(drive_mode, name, pivot_param_[i].joint_param[j].handle);
          if (result == controller_interface::CallbackReturn::ERROR) {
            DEBUG_ERROR_OUT("Get " << pivot_param_[i].joint_param[j].name << "handle joint false!");
            return controller_interface::CallbackReturn::FAILURE;
          }
        }
      }                
    }
  }  
 
  if (!drivers_state_ready_) {
    bool enable = true;
    if (activation_timer_) {
      activation_timer_->cancel();
      activation_timer_.reset();
    }
   
    activation_timer_ = get_node()->create_wall_timer(
      std::chrono::milliseconds(200),
      [this, enable](void) { enable_motor_devices(enable); },
      callback_group_); 
  }

  return controller_interface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn ChassisController::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  DEBUG_STREAM_OUT("on_deactivate");
  // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
  // instead of a loop
  for (size_t i = 0; i < hardware_interface_count_; ++i) {
    command_interfaces_[i].set_value(std::numeric_limits<double>::quiet_NaN());
  }

  // get hardware_interface handler
  for (int i = 0; i < pivot_num_; ++i)
  {   
    //获取编码器joint_handler
    if (!pivot_param_[i].external_encoder_name.empty() && pivot_param_[i].use_external_encoder ) {
      DEBUG_STREAM_OUT(
        "Delet encoder interface with joint name: " << pivot_param_[i].external_encoder_name);
      pivot_param_[i].encoder_handle.clear();
    }

    //获取电机joint_handler
    for (int j=0; j<2; ++j)
    {
      if (!pivot_param_[i].joint_param[j].name.empty()) {
        DEBUG_STREAM_OUT(
            "Delet wheel interface with joint name: " << pivot_param_[i].joint_param[j].name);

        if (pivot_param_[i].joint_param[j].drive_mode == TRACTION
            || pivot_param_[i].joint_param[j].drive_mode == STEER) {
          pivot_param_[i].joint_param[j].handle.clear();
        }
      }                
    }
  }    
  
  if (drivers_state_ready_) {
    bool enable = false;
    if (activation_timer_) {
      activation_timer_->cancel();
      activation_timer_.reset();
    }

    activation_timer_ = get_node()->create_wall_timer(
      std::chrono::milliseconds(200),
      [this, enable](void) { enable_motor_devices(enable); },
      callback_group_); 
  }

  return controller_interface::CallbackReturn::SUCCESS;
}

controller_interface::return_type ChassisController::update(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
  // instead of a loop
  updatePivotValue();

  updateOdometry(time);

  updateCommand(time, period);
  
  if (chassis_type_ == MULTI_PIVOTS && monitor_param_.enable_current_monitor){
      current_monitor_.update(pivot_value_, pivot_param_, event_msg_,standstill_, reset_button_, monitor_param_);
  }
  pubError();

  return controller_interface::return_type::OK;
}


controller_interface::CallbackReturn ChassisController::configure_side(
  const enum joint_type & wheel_kind,
  const std::string & joint_name,
  GenericHandle & registered_handles)
{
  if (joint_name.empty()) {
    DEBUG_ERROR_OUT("No  motor names specified: " << joint_name);
    return controller_interface::CallbackReturn::ERROR;
  }

  auto velocity_cmd_it = std::find_if(
    command_interfaces_.begin(), command_interfaces_.end(),
    [&joint_name](const auto& interface) {
    return interface.get_prefix_name() == joint_name &&
    interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY;
  });

  auto position_cmd_it = std::find_if(
      command_interfaces_.begin(), command_interfaces_.end(),
      [&joint_name](const auto& interface) {
      return interface.get_prefix_name() == joint_name &&
      interface.get_interface_name() == hardware_interface::HW_IF_POSITION;
  });

  auto velocity_state_it = std::find_if(
      state_interfaces_.cbegin(), state_interfaces_.cend(),
      [&joint_name](const auto & interface) {
      return interface.get_prefix_name() == joint_name &&
      interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY;
  });

  auto position_state_it = std::find_if(
      state_interfaces_.cbegin(), state_interfaces_.cend(),
      [&joint_name](const auto & interface) {
      return interface.get_prefix_name() == joint_name &&
      interface.get_interface_name() == hardware_interface::HW_IF_POSITION;
  });

  if (velocity_state_it != state_interfaces_.cend()) {
    DEBUG_STREAM_OUT("Found interfaces for velocity joint: " << joint_name.c_str());
    registered_handles.joint_state_interfaces.push_back(*velocity_state_it);
  } else {
    DEBUG_ERROR_OUT("Missing interfaces for velocity joint: " << joint_name.c_str());
    return CallbackReturn::ERROR;
  }

  if (position_state_it != state_interfaces_.cend()) {
    DEBUG_STREAM_OUT("Found interfaces for position joint: " << joint_name.c_str());
    registered_handles.joint_state_interfaces.push_back(*position_state_it);
  } else {
    DEBUG_ERROR_OUT("Missing interfaces for position joint: " << joint_name.c_str());
    return CallbackReturn::ERROR;
  }

  if (wheel_kind != ENCODER) {
    auto torque_state_it = std::find_if(
      state_interfaces_.cbegin(), state_interfaces_.cend(),
      [&joint_name](const auto & interface) {
      return interface.get_prefix_name() == joint_name &&
      interface.get_interface_name() == hardware_interface::HW_IF_EFFORT;
    });

    if (torque_state_it != state_interfaces_.cend()) {
      DEBUG_STREAM_OUT("Found interfaces for effort joint: " << joint_name.c_str());
      registered_handles.joint_state_interfaces.push_back(*torque_state_it);
    } else {
      DEBUG_ERROR_OUT("Missing interfaces for effort joint: " << joint_name.c_str());
      return CallbackReturn::ERROR;
    }

    if (velocity_cmd_it != command_interfaces_.end()) {
      DEBUG_STREAM_OUT("Found interfaces for velocity command interface: " << joint_name);
      registered_handles.joint_command_interfaces.push_back(*velocity_cmd_it);
    } else {
        DEBUG_ERROR_OUT("Missing interfaces for command interface: " << joint_name);
        return CallbackReturn::ERROR;
    }

    if (position_cmd_it != command_interfaces_.end()) {
      DEBUG_STREAM_OUT("Found interfaces for position command interface: " << joint_name);
      registered_handles.joint_command_interfaces.push_back(*position_cmd_it);
    } else {
      DEBUG_ERROR_OUT("Missing position interface for command interface: " << joint_name);
      return CallbackReturn::ERROR;
    }
  }

  return controller_interface::CallbackReturn::SUCCESS;
}

bool ChassisController::config()
{   
  brake_operate_time_ = get_node()->now();
  last_state_publish_time_ = brake_operate_time_;
  
  try {
    if (!get_node()->get_parameter("pivots_num", pivot_num_) || pivot_num_ < 0) {
      DEBUG_WARN_OUT("Parameter pivot number error");
      return false;
    }
  } catch (const rclcpp::exceptions::ParameterNotDeclaredException& e) {
    DEBUG_WARN_OUT("Parameter joint number does not exist: " << e.what());
    return false;
  }

  pivot_param_.resize(pivot_num_);

  for (int i = 0; i < pivot_num_; ++i) {
    pivot_param_[i].joint_param.resize(2);
  }

  DEBUG_STREAM_OUT("pivot param vector resized");

  try {
    if (!get_node()->get_parameter("chassis_type", chassis_type_) || chassis_type_ < 0) {
      DEBUG_WARN_OUT("chassis type error");
      return false;
    }
  } catch (const rclcpp::exceptions::ParameterNotDeclaredException& e) {
      DEBUG_WARN_OUT("Parameter chassis type does not exist: " << e.what());
      return false;
  }
  
  double maxload;
  double minload;
  double maxfactor;
  double minfactor;
 
  dynamic_odom_.setMaxLoad(get_node()->get_parameter_or("odometry_compensation/max_load", maxload, 1.0));
  dynamic_odom_.setMinLoad(get_node()->get_parameter_or("odometry_compensation/min_load", minload, 0.0));
  dynamic_odom_.setMaxFactor(get_node()->get_parameter_or("odometry_compensation/max_factor", maxfactor, 1.0));
  dynamic_odom_.setMinFactor(get_node()->get_parameter_or("odometry_compensation/min_factor", minfactor, 1.0));

  //### need cheek ###
  get_node()->get_parameter_or("/IO/output_device/brakeOut", braker_index_, -1); 

  kinematicsPtr_ = Kinematics::create(get_node(), chassis_type_);
  DEBUG_OUT("kinematics instantiation created. chassis_type: %d", chassis_type_);
  odometryPtr_ = Odometry::create(get_node(), chassis_type_);
  DEBUG_STREAM_OUT("odometry instantiation created");

  get_node()->get_parameter_or("brake_delay_time", brake_delay_time_, 0.5);
  DEBUG_OUT("get brake delay time: %.4fs.", brake_delay_time_);

  get_node()->get_parameter_or("enable_brake_control", enable_brake_control_, false);
  DEBUG_OUT("get enable_brake_control: %d.", enable_brake_control_);

  get_node()->get_parameter_or("enable_ekf", use_ekf_with_imu_, false);
  DEBUG_OUT("get enable_ekf: %d.", use_ekf_with_imu_);

  get_node()->get_parameter_or("use_pdo_timestamp", use_pdo_timestamp_, false);
  DEBUG_OUT("get use_pdo_timestamp: %d.", use_pdo_timestamp_);

  get_node()->get_parameter_or("enable_current_monitor", monitor_param_.enable_current_monitor, false);
  DEBUG_STREAM_OUT(
      "get enable_current_monitor: "<< monitor_param_.enable_current_monitor);

  get_node()->get_parameter_or("current_monitor_time", monitor_param_.current_monitor_time, 20);
  DEBUG_STREAM_OUT(
      "get current_monitor_time: "<< monitor_param_.current_monitor_time); 

  get_node()->get_parameter_or("imbalance_threshold", monitor_param_.imbalance_threshold, 2.0);
  DEBUG_STREAM_OUT("get imbalance_threshold: "<<monitor_param_.imbalance_threshold); 


  dynamic_pivot_params_.wheel_diameter.resize(pivot_num_);
  dynamic_pivot_params_.radius_ringmount.resize(pivot_num_);
  dynamic_pivot_params_.steer_offset.resize(pivot_num_);
  dynamic_pivot_params_.offset_x.resize(pivot_num_);
  dynamic_pivot_params_.offset_y.resize(pivot_num_);
  // dynamic_pivot_params_.left_wheel_diameter.resize(pivot_num_);
  // dynamic_pivot_params_.right_wheel_diameter.resize(pivot_num_);

  /*根据驱动单元数量获取各关节的参数*/
  // 从参数服务器读取到各关节的名字
  std::string temp_name, temp_name2;

  for (int i = 0; i < pivot_num_; ++i) {
    temp_name = "pivot_param" + std::to_string(i + 1) + "/";

    get_node()->get_parameter_or(temp_name + "name", pivot_param_[i].name, pivot_param_[i].name);
    get_node()->get_parameter_or(temp_name + "pivot_type", pivot_param_[i].pivot_type, pivot_param_[i].pivot_type);
    get_node()->get_parameter_or(temp_name + "driving_wheel_diameter", dynamic_pivot_params_.wheel_diameter[i], dynamic_pivot_params_.wheel_diameter[i]);
    get_node()->get_parameter_or(temp_name + "radius_ringmount", dynamic_pivot_params_.radius_ringmount[i], dynamic_pivot_params_.radius_ringmount[i]);
    get_node()->get_parameter_or(temp_name + "steering_ratio", pivot_param_[i].steering_ratio, pivot_param_[i].steering_ratio);
    get_node()->get_parameter_or(temp_name + "traction_ratio", pivot_param_[i].traction_ratio, pivot_param_[i].traction_ratio);
    get_node()->get_parameter_or(temp_name + "slewing_gear", pivot_param_[i].slewing_gear, pivot_param_[i].slewing_gear);
    get_node()->get_parameter_or(temp_name + "drive_gear", pivot_param_[i].drive_gear, pivot_param_[i].drive_gear);
    get_node()->get_parameter_or(temp_name + "feedback_gear", pivot_param_[i].feedback_gear, pivot_param_[i].feedback_gear);
    get_node()->get_parameter_or(temp_name + "steer_offset", dynamic_pivot_params_.steer_offset[i], dynamic_pivot_params_.steer_offset[i]);
    get_node()->get_parameter_or(temp_name + "velocity_factor", pivot_param_[i].velocity_factor, pivot_param_[i].velocity_factor);
    get_node()->get_parameter_or(temp_name + "odometry_factor", pivot_param_[i].odometry_factor, 1.0);
    get_node()->get_parameter_or(temp_name + "use_external_encoder", pivot_param_[i].use_external_encoder, pivot_param_[i].use_external_encoder);
    get_node()->get_parameter_or(temp_name + "steer_feed_invert", pivot_param_[i].steer_feed_invert, pivot_param_[i].steer_feed_invert);
    get_node()->get_parameter_or(temp_name + "external_encoder_resolution", pivot_param_[i].external_encoder_resolution, pivot_param_[i].external_encoder_resolution);
    get_node()->get_parameter_or(temp_name + "external_encoder_name", pivot_param_[i].external_encoder_name, pivot_param_[i].external_encoder_name);
    get_node()->get_parameter_or(temp_name + "offset_x", dynamic_pivot_params_.offset_x[i], dynamic_pivot_params_.offset_x[i]);
    get_node()->get_parameter_or(temp_name + "offset_y", dynamic_pivot_params_.offset_y[i], dynamic_pivot_params_.offset_y[i]);
    get_node()->get_parameter_or(temp_name + "max_angle", pivot_param_[i].max_angle, 1.7);
    get_node()->get_parameter_or(temp_name + "min_angle", pivot_param_[i].min_angle, -1.7);

    for (size_t j = 0; j < 2; ++j) {
      temp_name2 = temp_name + "joint" + std::to_string(j + 1) + "/";
      bool tempOK = get_node()->get_parameter_or(temp_name2 + "name", 
        pivot_param_[i].joint_param[j].name, pivot_param_[i].joint_param[j].name);
      DEBUG_STREAM_OUT("get joint name ok ?" 
        << tempOK << "," << i << "," << j << " name:" << temp_name2 << pivot_param_[i].joint_param[j].name);
      // get_node()->get_parameter_or(temp_name2 + "modle", pivot_param_[i].joint_param[j].modle, pivot_param_[i].joint_param[j].modle);
      // get_node()->get_parameter_or(temp_name2 + "ID", pivot_param_[i].joint_param[j].ID, pivot_param_[i].joint_param[j].ID);
      get_node()->get_parameter_or(temp_name2 + "drive_mode", pivot_param_[i].joint_param[j].drive_mode, pivot_param_[i].joint_param[j].drive_mode);
      get_node()->get_parameter_or(temp_name2 + "resolution", pivot_param_[i].joint_param[j].resolution, pivot_param_[i].joint_param[j].resolution);
      moter_names_.push_back(pivot_param_[i].joint_param[j].name);
    }
  }

  moter_num_ = moter_names_.size();
  pivot_value_.resize(pivot_num_); 
  pivot_setpoint_.resize(pivot_num_);

  for (int i = 0; i < pivot_num_; ++i) {
    pivot_value_[i].joint_value.resize(2);
    pivot_value_[i].joint_value[0].velocity = 0.0;
    pivot_value_[i].joint_value[1].velocity = 0.0;
    pivot_value_[i].joint_value[0].position = 0.0;
    pivot_value_[i].joint_value[1].position = 0.0;
  }

  for (int i = 0; i < pivot_num_; ++i) {
    pivot_setpoint_[i].joint_setpoint.resize(2);
    pivot_setpoint_[i].joint_setpoint[0].velocity_setpoint = 0.0;
    pivot_setpoint_[i].joint_setpoint[1].velocity_setpoint = 0.0;
    pivot_setpoint_[i].joint_setpoint[0].position_setpoint = 0.0;
    pivot_setpoint_[i].joint_setpoint[1].position_setpoint = 0.0;
  }

  get_node()->get_parameter_or("steer_offset1", steer_offset1_, 0.0);
  get_node()->get_parameter_or("steer_offset2", steer_offset2_, 0.0);
  get_node()->get_parameter_or("steer_offset3", steer_offset3_, 0.0);
  get_node()->get_parameter_or("steer_offset4", steer_offset4_, 0.0);
  get_node()->get_parameter_or("steer_offset5", steer_offset5_, 0.0);
  get_node()->get_parameter_or("steer_offset6", steer_offset6_, 0.0);
  get_node()->get_parameter_or("steer_offset7", steer_offset7_, 0.0);
  get_node()->get_parameter_or("steer_offset8", steer_offset8_, 0.0);        
  get_node()->get_parameter_or("offset_y_modification", offset_y_, 0.0);
  get_node()->get_parameter_or("wheel_diameter_multiplier", wheel_diameter_multiplier_, 1.0);
  get_node()->get_parameter_or("pivot_offset_multiplier", pivot_offset_multiplier_, 1.0);
  get_node()->get_parameter_or("ring_mount_multiplier", ring_mount_multiplier_, 1.0);
  get_node()->get_parameter_or("left_wheel_radius_multiplier", left_wheel_radius_multiplier_, 1.0);
  get_node()->get_parameter_or("right_wheel_radius_multiplier", right_wheel_radius_multiplier_, 1.0);

  steer_offset_addend.push_back(steer_offset1_);
  steer_offset_addend.push_back(steer_offset2_);
  steer_offset_addend.push_back(steer_offset3_);
  steer_offset_addend.push_back(steer_offset4_);
  steer_offset_addend.push_back(steer_offset5_);
  steer_offset_addend.push_back(steer_offset6_);
  steer_offset_addend.push_back(steer_offset7_);
  steer_offset_addend.push_back(steer_offset8_);

  // Odometry related:
  double publish_rate;
  get_node()->get_parameter_or("publish_rate", publish_rate, 50.0);
  DEBUG_STREAM_OUT(
      "Controller state will be published at "
      << publish_rate << "Hz.");
  publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate);

  get_node()->get_parameter_or("open_loop", open_loop_, false);

  // Twist command related:
  get_node()->get_parameter_or("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
  DEBUG_STREAM_OUT(
      "Velocity commands will be considered old if they are older than "
      << cmd_vel_timeout_ << "s.");

  get_node()->get_parameter_or("base_frame_id", base_frame_id_, base_frame_id_);
  DEBUG_STREAM_OUT("Base frame_id set to " << base_frame_id_);

  get_node()->get_parameter_or("odom_frame_id", odom_frame_id_, odom_frame_id_);
  DEBUG_STREAM_OUT("Odometry frame_id set to " << odom_frame_id_);

  get_node()->get_parameter_or("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
  DEBUG_STREAM_OUT("Publishing to tf is "
      << (enable_odom_tf_ ? "enabled" : "disabled"));

  get_node()->get_parameter_or("filter_window_size", filter_window_, 3);
  
  // Velocity and acceleration limits:
  get_node()->get_parameter_or("linear/x/has_velocity_limits", limiter_lin_.has_velocity_limits, limiter_lin_.has_velocity_limits);
  get_node()->get_parameter_or("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
  get_node()->get_parameter_or("linear/x/max_velocity", limiter_lin_.max_velocity, limiter_lin_.max_velocity);
  get_node()->get_parameter_or("linear/x/min_velocity", limiter_lin_.min_velocity, -limiter_lin_.max_velocity);
  get_node()->get_parameter_or("linear/x/max_acceleration", limiter_lin_.max_acceleration, limiter_lin_.max_acceleration);
  get_node()->get_parameter_or("linear/x/min_acceleration", limiter_lin_.min_acceleration, -limiter_lin_.max_acceleration);

  get_node()->get_parameter_or("angular/z/has_velocity_limits", limiter_ang_.has_velocity_limits, limiter_ang_.has_velocity_limits);
  get_node()->get_parameter_or("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
  get_node()->get_parameter_or("angular/z/max_velocity", limiter_ang_.max_velocity, limiter_ang_.max_velocity);
  get_node()->get_parameter_or("angular/z/min_velocity", limiter_ang_.min_velocity, -limiter_ang_.max_velocity);
  get_node()->get_parameter_or("angular/z/max_acceleration", limiter_ang_.max_acceleration, limiter_ang_.max_acceleration);
  get_node()->get_parameter_or("angular/z/min_acceleration", limiter_ang_.min_acceleration, -limiter_ang_.max_acceleration);

  get_node()->get_parameter_or("steer/has_velocity_limits", steer_limiter_.has_velocity_limits, steer_limiter_.has_velocity_limits);
  get_node()->get_parameter_or("steer/has_acceleration_limits", steer_limiter_.has_acceleration_limits, steer_limiter_.has_acceleration_limits);
  get_node()->get_parameter_or("steer/has_jerk_limits", steer_limiter_.has_jerk_limits, steer_limiter_.has_jerk_limits);
  get_node()->get_parameter_or("steer/max_velocity", steer_limiter_.max_velocity, steer_limiter_.max_velocity);
  get_node()->get_parameter_or("steer/min_velocity", steer_limiter_.min_velocity, -steer_limiter_.max_velocity);
  get_node()->get_parameter_or("steer/max_acceleration", steer_limiter_.max_acceleration, steer_limiter_.max_acceleration);
  get_node()->get_parameter_or("steer/min_acceleration", steer_limiter_.min_acceleration, -steer_limiter_.max_acceleration);
  get_node()->get_parameter_or("steer/max_jerk", steer_limiter_.max_jerk, steer_limiter_.max_jerk);
  get_node()->get_parameter_or("steer/min_jerk", steer_limiter_.min_jerk, -steer_limiter_.max_jerk);
  // steer control
  get_node()->get_parameter_or("Kp", steer_pid_param_.Kp, 0.5);
  get_node()->get_parameter_or("Ki", steer_pid_param_.Ki, 0.05);
  get_node()->get_parameter_or("Kd", steer_pid_param_.Kd, 0.0);
  get_node()->get_parameter_or("integral_limit", steer_pid_param_.integral_limit, 0.2);
  get_node()->get_parameter_or("integral_threshold", steer_pid_param_.integral_threshold, 0.2);
  get_node()->get_parameter_or("upper_limit", steer_pid_param_.upper_limit, 1.0);
  get_node()->get_parameter_or("lower_limit", steer_pid_param_.lower_limit, -1.0);
  get_node()->get_parameter_or("inverted", steer_pid_param_.inverted, false);

  output_client_ = get_node()->create_client<agv_srvs::srv::OutputInterface>("/outputinterface");
  motor_enable_client_ = get_node()->create_client<agv_srvs::srv::MotorEnable>("/motorEnable");

  event_pub_ = get_node()->create_publisher<agv_msgs::msg::AGVEvent>("/agvevent", 10); 
 
  sub_readinputnew_ = get_node()->create_subscription<agv_msgs::msg::ReadInPutsNew>(
          "/readInputNew", 10, std::bind(&ChassisController::readInputCallback, this, std::placeholders::_1));

  steer_direction_ = get_node()->create_subscription<agv_msgs::msg::DoSteerDir>(
          "/doSteerDir", 1, std::bind(&ChassisController::steerDirCallback, this, std::placeholders::_1));

  // sub_gross_load_  = get_node()->create_subscription<agv_msgs::msg::GrossLoad>(
  //         "/grossLoad", 1, std::bind(&ChassisController::grossLoadCallback, this, std::placeholders::_1));  

  DEBUG_STREAM_OUT("cmdVelCallback ");
  kinematicsPtr_->init(pivot_num_, steer_pid_param_, limiter_lin_, limiter_ang_, steer_limiter_);
  DEBUG_STREAM_OUT("kinematicsPtr init ");
  current_monitor_.init(get_node(), event_pub_, get_node()->now(), pivot_value_);
  
  callback_group_ = get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

  parameter_callback_handle_ = get_node()->add_on_set_parameters_callback(
      std::bind(&ChassisController::parametersCallback, this, std::placeholders::_1));

  update_dynamic_parameters();
  return true;
}

void ChassisController::updatePivotValue()
{   
  if (chassis_type_ == DIFF) {
    pivot_param_[0].joint_param[0].wheel_diameter = 
        pivot_param_[0].driving_wheel_diameter * left_wheel_radius_multiplier_;

    pivot_param_[0].joint_param[1].wheel_diameter = 
        pivot_param_[0].driving_wheel_diameter * right_wheel_radius_multiplier_;
  }

  // 更新各驱动的实际值
  for (int i = 0; i < pivot_num_; ++i)
  {  //查找外置编码器的名字
    if (pivot_param_[i].use_external_encoder)
    {
      if (!pivot_param_[i].steer_feed_invert){
          pivot_value_[i].external_angle = 
                  pivot_param_[i].encoder_handle.joint_state_interfaces[0].get().get_value() - 
                  pivot_param_[i].steer_offset;
          DEBUG_OUT_THROTTLE(get_clock(), 
                  500, "pivot "<< i <<": steer raw data:"<< 
                  pivot_param_[i].encoder_handle.joint_state_interfaces[1].get().get_value());
          DEBUG_OUT_THROTTLE(get_clock(), 
                  500, "steer offset:" <<pivot_param_[i].steer_offset );
          DEBUG_OUT_THROTTLE(get_clock(), 
                  500, "external angle :"<< pivot_value_[i].external_angle);                
      } else {
          pivot_value_[i].external_angle = 
                  -(pivot_param_[i].encoder_handle.joint_state_interfaces[0].get().get_value() - 
                  pivot_param_[i].steer_offset);              
          DEBUG_OUT_THROTTLE(get_clock(), 
                  500, "pivot "<< i <<": steer raw data:"<< 
                  pivot_param_[i].encoder_handle.joint_state_interfaces[1].get().get_value());
          DEBUG_OUT_THROTTLE(get_clock(), 
                  500, "steer offset:" <<pivot_param_[i].steer_offset );
          DEBUG_OUT_THROTTLE(get_clock(), 
                  500, "external angle :"<< pivot_value_[i].external_angle);
      }
    }

    for (int j = 0; j < 2; ++j)
    {
      if (pivot_param_[i].pivot_type == DIFFWHEEL && chassis_type_ != DIFF ) {
        pivot_value_[i].joint_value[j].velocity = 
                pivot_param_[i].joint_param[j].handle.joint_state_interfaces[0].get().get_value() * 
                0.5 * pivot_param_[i].driving_wheel_diameter;   

        pivot_value_[i].joint_value[j].position = 
                pivot_param_[i].joint_param[j].handle.joint_state_interfaces[1].get().get_value() * 
                0.5 * pivot_param_[i].driving_wheel_diameter;

        pivot_value_[i].joint_value[j].current = 
                pivot_param_[i].joint_param[j].handle.joint_state_interfaces[2].get().get_value();

        DEBUG_OUT_THROTTLE(get_clock(), 
                500,"[DIFFWHEEL] pivot:" << i << "joint:" << j << "velocity =" << 
                pivot_value_[i].joint_value[j].velocity);
        DEBUG_OUT_THROTTLE(get_clock(), 
                500,"[DIFFWHEEL] pivot:" << i << "joint:" << j << "position =" << 
                pivot_value_[i].joint_value[j].position);

      } else if (pivot_param_[i].pivot_type == STEERWHEEL && 
        pivot_param_[i].joint_param[j].drive_mode == TRACTION) {

        pivot_value_[i].velocity = 
                pivot_param_[i].joint_param[j].handle.joint_state_interfaces[0].get().get_value() * 
                0.5 * pivot_param_[i].driving_wheel_diameter;    

        pivot_value_[i].position = 
                pivot_param_[i].joint_param[j].handle.joint_state_interfaces[1].get().get_value() * 
                0.5 * pivot_param_[i].driving_wheel_diameter;

        pivot_value_[i].current = 
                pivot_param_[i].joint_param[j].handle.joint_state_interfaces[2].get().get_value();

        DEBUG_OUT_THROTTLE(get_clock(), 
                500,"[STEERWHEEL] pivot:" << i << "velocity =" << pivot_value_[i].velocity);

        DEBUG_OUT_THROTTLE(get_clock(), 
                500,"[STEERWHEEL] pivot:" << i << "position =" << pivot_value_[i].position);

      } else if (pivot_param_[i].pivot_type == STEERWHEEL && 
          pivot_param_[i].joint_param[j].drive_mode == STEER) {

        if (!pivot_param_[i].steer_feed_invert)
            pivot_value_[i].steer_position = 
                    pivot_param_[i].joint_param[j].handle.joint_state_interfaces[1].get().get_value() - 
                        pivot_param_[i].steer_offset;
        else
            pivot_value_[i].steer_position = 
                    -(pivot_param_[i].joint_param[j].handle.joint_state_interfaces[1].get().get_value() - 
                        pivot_param_[i].steer_offset);

        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[STEERWHEEL] pivot:" << i << "steer position =" << pivot_value_[i].position);

      } else if (chassis_type_ == DIFF) {

        pivot_value_[0].joint_value[j].velocity =
                pivot_param_[i].joint_param[j].handle.joint_state_interfaces[0].get().get_value() * 
                    0.5 * pivot_param_[0].joint_param[j].wheel_diameter;

        pivot_value_[0].joint_value[j].position = 
                pivot_param_[i].joint_param[j].handle.joint_state_interfaces[1].get().get_value() *
                    0.5 * pivot_param_[0].joint_param[j].wheel_diameter;

        DEBUG_OUT_THROTTLE(get_clock(), 
                500,"[DIFF] joint:"  << j << " velocity = " << 
                    pivot_value_[0].joint_value[j].velocity);

        DEBUG_OUT_THROTTLE(get_clock(), 
                500,"[DIFF] joint:"  << j << " position = " << 
                    pivot_value_[0].joint_value[j].position);
      }
    }
  }
}

void ChassisController::updateOdometry(const rclcpp::Time &time)
{   
  if (standstill_){
    odom_factor_ = dynamic_factor_;
  }

  DEBUG_OUT_THROTTLE(get_clock(), 
      500, "valid odometry factor is:" << odom_factor_);

  odometryPtr_->update(pivot_value_, pivot_param_, time, standstill_, odom_factor_);

  DEBUG_OUT_THROTTLE(get_clock(), 
      500, 
      "Odom: x:"<< odometryPtr_->getX() << " y:" << odometryPtr_->getY() << "heading:" << odometryPtr_->getHeading());

  // Publish odometry message
  if (last_state_publish_time_ + publish_period_ < time) {
    last_state_publish_time_ += publish_period_;
    tf2::Quaternion orientation;
    orientation.setRPY(0, 0, odometryPtr_->getHeading());
        
    // Populate odom message and publish
    if (rt_odom_state_publisher_->trylock()) {
      // if (use_pdo_timestamp_) {
      //     for (auto& jh : used_joint_handle_vector_) {
      //         if (jh != nullptr) {
      //             if (jh->getTimestamp() < 0.0001) {
      //                 RCLCPP_WARN_THROTTLE(get_node()->get_logger(), get_clock(), 
      //                     1000.0, "%s has bad timstamp: %.6f", jh->getName().c_str(), jh->getTimestamp());
      //             }

      //             if (jh->getTimestamp() < rt_odom_state_publisher_->msg_.header.stamp.seconds()) {
      //                 rt_odom_state_publisher_->msg_.header.stamp = rclcpp::Time(jh->getTimestamp());
      //             }
      //         }
      //     }
      //     DEBUG_OUT_THROTTLE(get_node()->get_logger(), get_clock(), 
      //         500, "odom time diff: %.6f", (time - odom_pub_->msg_.header.stamp).seconds());
      // }
      auto & odometry_message = rt_odom_state_publisher_->msg_;
      odometry_message.header.stamp = time;
      odometry_message.pose.position.x = odometryPtr_->getX();
      odometry_message.pose.position.y = odometryPtr_->getY();
      odometry_message.pose.orientation.x = orientation.x();
      odometry_message.pose.orientation.y = orientation.y();
      odometry_message.pose.orientation.z = orientation.z();
      odometry_message.pose.orientation.w = orientation.w();
      odometry_message.twist.linear.x = odometryPtr_->getVx();
      odometry_message.twist.linear.y = odometryPtr_->getVy();
      odometry_message.twist.angular.z = odometryPtr_->getVa();
      rt_odom_state_publisher_->unlockAndPublish();
    }

    // Publish tf /odom frame
    if (enable_odom_tf_ && rt_tf_odom_state_publisher_->trylock()) {
      auto & transform = rt_tf_odom_state_publisher_->msg_.transforms.front();
      transform.header.stamp = time;
      transform.transform.translation.x = odometryPtr_->getX();
      transform.transform.translation.y = odometryPtr_->getY();
      transform.transform.rotation.x = orientation.x();
      transform.transform.rotation.y = orientation.y();
      transform.transform.rotation.z = orientation.z();
      transform.transform.rotation.w = orientation.w();
      rt_tf_odom_state_publisher_->unlockAndPublish();
    }
  }
}

void ChassisController::updateCommand(const rclcpp::Time &time,
                                      const rclcpp::Duration &period)
{
  // Retreive current velocity command and time step:
  const auto curr_cmd_ = *(input_ref_.readFromRT());

  if (curr_cmd_ == nullptr) {
      DEBUG_WARN_OUT("Velocity message received was a nullptr.");
      return;
  }

  const double dt = (time - curr_cmd_->header.stamp).seconds();
  const bool timeout = dt > cmd_vel_timeout_;
  if (!timeout && (std::isnan(curr_cmd_->twist.linear.x) || 
      std::isnan(curr_cmd_->twist.linear.y) ||
      std::isnan(curr_cmd_->twist.angular.z))) {
    return;
  } else if (timeout) {
    curr_cmd_->twist.linear.x = 0.0;
    curr_cmd_->twist.linear.y = 0.0;
    curr_cmd_->twist.angular.z = 0.0;
  }

  DEBUG_WARN_ON_TRANSITION(timeout, "cmd_vel timeout, set linear and angular velocity zero.");
  // DEBUG_STREAM_OUT("curr_cmd_ x:" <<  curr_cmd_->twist.linear.x << 
  //   ",y:" << curr_cmd_->twist.linear.y << 
  //   ",a:" << curr_cmd_->twist.angular.z);
  const double cmd_dt(period.seconds());
  // const double angular_speed = odometryPtr_.getAngular();
  // DEBUG_OUT("cmd_dt :" << cmd_dt);
  double curr_cmd_liner = sqrt(pow(curr_cmd_->twist.linear.x, 2) + pow(curr_cmd_->twist.linear.y, 2));
  double curr_cmd_angular = curr_cmd_->twist.angular.z;

  // Limit velocities and accelerations:
  double reduce_factor_liner, reduce_factor_angular;
  // use the minimun reduce factor
  reduce_factor_liner = limiter_lin_.limit(curr_cmd_liner, last0_cmd_liner_, last1_cmd_liner_, cmd_dt);
  reduce_factor_angular = limiter_ang_.limit(curr_cmd_angular, last0_cmd_angular_, last1_cmd_angular_, cmd_dt);
  // DEBUG_STREAM_OUT("reduce_factor_liner :" << reduce_factor_liner << "reduce_factor_angular :" << reduce_factor_angular);
 
  if (reduce_factor_liner >= reduce_factor_angular) {
    curr_cmd_->twist.linear.x *= reduce_factor_angular;
    curr_cmd_->twist.linear.y *= reduce_factor_angular;
    curr_cmd_->twist.angular.z *= reduce_factor_angular;
    last0_cmd_liner_ = curr_cmd_liner / reduce_factor_liner * reduce_factor_angular;
    last0_cmd_angular_ = curr_cmd_angular;
  } else {
    curr_cmd_->twist.linear.x *= reduce_factor_liner;
    curr_cmd_->twist.linear.y *= reduce_factor_liner;
    curr_cmd_->twist.angular.z *= reduce_factor_liner;
    last0_cmd_liner_ = curr_cmd_liner;
    last0_cmd_angular_ = curr_cmd_angular / reduce_factor_angular * reduce_factor_liner;
  }
  // save last value
  last1_cmd_liner_ = last0_cmd_liner_;
  last1_cmd_angular_ = last0_cmd_angular_;

  kinematicsPtr_->getVelocity(odometryPtr_->getVx(), odometryPtr_->getVy(), odometryPtr_->getVa());
  
  kinematicsPtr_->getCommand(curr_cmd_->twist.linear.x, curr_cmd_->twist.linear.y, curr_cmd_->twist.angular.z);

  kinematicsPtr_->setSteerCmdId(steer_cmd_id_);

  kinematicsPtr_->update(pivot_value_, pivot_param_, pivot_setpoint_, period, ready_move_);

  if (enable_brake_control_) 
    brakerControl();
  else 
    ready_move_ = true;
  
  kinematicsPtr_->setCommand(pivot_value_, pivot_param_, pivot_setpoint_, period);

  if (rt_streer_publisher_->trylock()) {
    rt_streer_publisher_->msg_.id = kinematicsPtr_->getSteerCmdId();
    rt_streer_publisher_->msg_.steering = kinematicsPtr_->getSteeringFlag();
    rt_streer_publisher_->msg_.steer_err = kinematicsPtr_->getSteerErrFlag();
    rt_streer_publisher_->msg_.steer_err = kinematicsPtr_->getSteerErrFlag();
    rt_streer_publisher_->msg_.chassis_not_ready = !ready_move_;
    rt_streer_publisher_->unlockAndPublish();
  }
  standstill_ = (abs(curr_cmd_->twist.linear.x)<0.0001 && abs(curr_cmd_->twist.linear.y)<0.0001 && abs(curr_cmd_->twist.angular.z)<0.0001);
}

bool ChassisController::enable_motor_devices(bool enable)
{
  std::lock_guard<std::mutex> lock(enable_mutex_);
  while (!motor_enable_client_->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
        DEBUG_OUT("Interrupted while waiting for service.");
        return false;
    }
    DEBUG_OUT("Service not available, waiting...");
  }

  if (moter_names_.empty()) {
    DEBUG_OUT("joint names empty. return");
    return false;
  }
  DEBUG_STREAM_OUT("## Call enable_motor_devices.");
  try {
    auto request = std::make_shared<agv_srvs::srv::MotorEnable::Request>();
    
    request->joint_names = moter_names_;
    request->enable = enable;
 
    // 发送异步请求
    auto response_future = motor_enable_client_->async_send_request(request);
    auto status = response_future.wait_for(std::chrono::milliseconds(5000));
    if (status == std::future_status::ready) {
      auto response = response_future.get();
      DEBUG_STREAM_OUT("successed call motor device enabled:" << (enable ? "enbale" : "disable"));
      activation_timer_->cancel();
      if (!enable)
        drivers_state_ready_ = false;
      return response->success; 
    } else {
      DEBUG_ERROR_OUT("call service failed");
      return false;
    }
  } catch (const std::exception &e) {
    DEBUG_STREAM_OUT("call service failed: error" << e.what());
    return false;
  }
}
    
void ChassisController::brakerControl()
{
  rclcpp::Time now = get_node()->now();

  if (futures_.size()) {
    bool ready = true;
    for (size_t i=0; i<futures_.size(); i++) {
        std::future_status status = futures_[i].wait_for(std::chrono::seconds(0));
        ready &= (status == std::future_status::ready);
        DEBUG_OUT("futures_[%ld] status: %d", i, (int)status);
    }
    
    DEBUG_OUT("futures ready: %d", (int)ready);

    bool result = false;
    bool finished = true;
    if (ready) {
      for (size_t i=0; i<futures_.size(); i++) {
        try {
            result = futures_[i].get();
        }
        catch (const std::future_error& e) {
            DEBUG_ERROR_OUT(
                "future " << i << " get result error: " << e.what());
            finished = false;
        }

        if (!result) {
            DEBUG_ERROR_OUT("async task failed!" << i);
            finished = false;
        }
        else {
            DEBUG_STREAM_OUT("async task succeed. " << i);
        }
      }

      futures_.clear();
      futures_.resize(0);
      
      if (finished) {
        if (joints_enable_check_) {
          joints_enable_ = true;
          auto request = std::make_shared<agv_srvs::srv::OutputInterface::Request>();
          auto index_level = request->index_level;
          agv_msgs::msg::OutputPort singleIo;

          singleIo.index =  braker_index_;
          singleIo.level = true;
          index_level.push_back(singleIo);
          
          DEBUG_OUT("call release brake.");
            try {
              using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::OutputInterface>::SharedFuture;
              auto response_received_callback = [this](ServiceResponseFuture future) {
                  (void)future;
                  DEBUG_OUT("call outputport service success." );
              };
              
              auto result = output_client_->async_send_request(request, response_received_callback);
          } catch (const std::exception &e) {
              DEBUG_ERROR_OUT("call outputport service error");
          }
          brake_operate_time_ = now;
        }
      } else {
        for (int i = 0; i < pivot_num_; i++) {
          for (int j = 0; j < 2; ++j) {
              std::string pivot_joint_name = pivot_param_[i].joint_param[j].name;
              assert (!pivot_joint_name.empty());

              pivot_param_[i].joint_param[j].handle.joint_command_interfaces[0].get().set_value(0);
              pivot_setpoint_[i].joint_setpoint[j].velocity_setpoint = 0.0;

              // if (braker_out_last_) {
              //     std::future<bool> fut = std::async(std::launch::async, 
              //         pivot_param_[i].joint_param[j].handle->getEnableFunc());
              //     futures_.emplace_back(std::move(fut));
              //     DEBUG_OUT(
              //         "enable joint again: %s", pivot_param_[i].joint_param[j].handle->getName().c_str());
              // }
              // else {
              //     std::future<bool> fut = std::async(std::launch::async, 
              //         pivot_param_[i].joint_param[j].handle->getDisableFunc());
              //     futures_.emplace_back(std::move(fut));
              //     DEBUG_OUT(
              //         "disable joint again: %s", pivot_param_[i].joint_param[j].handle->getName().c_str());
              // }
          }
        }
      }
    }
  }

  if (joints_enable_ && (now.seconds() - brake_operate_time_.seconds()) > brake_delay_time_) 
      ready_move_ = true;
  else 
      ready_move_ = false;
  

  DEBUG_OUT_THROTTLE(get_clock(), 
          500, "joints_enable:" << joints_enable_ << " ready_move_:" << ready_move_);

  bool brake_release = !vehicle_status_.emergency_stop && kinematicsPtr_->getBrakerFlag();
  if (brake_release == braker_out_last_)
      return;

  DEBUG_OUT("change brake state: %d", brake_release);
  braker_out_last_ = brake_release;
  auto request = std::make_shared<agv_srvs::srv::OutputInterface::Request>();
  auto index_level = request->index_level;
  agv_msgs::msg::OutputPort singleIo;

  singleIo.index =  braker_index_;
  singleIo.level = brake_release;
  index_level.push_back(singleIo);
 
  if (brake_release) {
      joints_enable_check_ = true;
  }
  else {
      joints_enable_check_ = false;
      ready_move_ = false;
      DEBUG_OUT("call hold brake.");
      try {
          using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::OutputInterface>::SharedFuture;
          auto response_received_callback = [this](ServiceResponseFuture future) {
              (void) future;
              DEBUG_OUT("call outputport service success." );
          };
          
          auto result = output_client_->async_send_request(request, response_received_callback);
      } catch (const std::exception &e) {
          DEBUG_ERROR_OUT("call outputport service error");
      }
     
      brake_operate_time_ = now;
  }

  joints_enable_ = false;

  futures_.resize(0);
  for (int i = 0; i < pivot_num_; i++) {
    for (int j = 0; j < 2; ++j) {
      std::string pivot_joint_name = pivot_param_[i].joint_param[j].name;
      assert (!pivot_joint_name.empty());

      pivot_param_[i].joint_param[j].handle.joint_command_interfaces[0].get().set_value(0);
      pivot_setpoint_[i].joint_setpoint[j].velocity_setpoint = 0.0;

      // if (brake_release) {
      //     std::future<bool> fut = std::async(std::launch::async, 
      //         pivot_param_[i].joint_param[j].handle->getEnableFunc());
      //     futures_.emplace_back(std::move(fut));
      //     DEBUG_OUT(
      //         "enable joint: %s", pivot_param_[i].joint_param[j].handle->getName().c_str());
      // }
      // else {
      //     std::future<bool> fut = std::async(std::launch::async, 
      //         pivot_param_[i].joint_param[j].handle->getDisableFunc());
      //     futures_.emplace_back(std::move(fut));
      //     DEBUG_OUT(
      //         "disable joint: %s", pivot_param_[i].joint_param[j].handle->getName().c_str());
      // }
    }
  }
}

void ChassisController::steerDirCallback(const agv_msgs::msg::DoSteerDir &steer_dir)
{
  steer_cmd_id_ = steer_dir.id;
  for (int i = 0; i < pivot_num_; ++i) {
    pivot_setpoint_[i].invert = (steer_dir.steer_dir == 1);
    pivot_setpoint_[i].forbidden_invert = (steer_dir.steer_dir == 2);
    pivot_setpoint_[i].suspend_invert = steer_dir.prohibit_limit_flip;
  }
}

void ChassisController::readInputCallback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg)
{
  std::vector<agv_msgs::msg::PairTypeInt> input_msg = msg->array;
      
  bool e_stop = false;
  for (const auto &data : input_msg) {
    if (data.key.compare(0, 15, "emergencyButton") == 0) {
      e_stop |= (data.value == 1);
    }

    if (data.key == "resetButton") {
      reset_button_ = (data.value == 1);
    }
  }

  vehicle_status_.emergency_stop = e_stop;
}

void ChassisController::drivers_info_callback(const agv_msgs::msg::DriversInfo &driver_info)
{
  bool drivers_state = true;
  bool find_device = true;
  using DRIVERSTATE = agv_msgs::msg::DriverState;
  std::unordered_map<std::string, const DRIVERSTATE*> driver_joint_map;
  for (const auto& driver_state : driver_info.drivers) {
    driver_joint_map[driver_state.jointname] = &driver_state; 
  }

  for (int i = 0; i < pivot_num_; ++i) {
    for (int j = 0; j < 2; ++j) {
      const auto& joint_param = pivot_param_[i].joint_param[j];
      if (joint_param.name.empty()) continue;
      auto it = driver_joint_map.find(joint_param.name);
      if (it != driver_joint_map.end()) {
        const DRIVERSTATE* driver_state = it->second;
        drivers_state &= driver_state->ready; 
      } else {
        find_device = false;
        break;
      }
    }
  }

  drivers_state_ready_ = drivers_state && find_device;
}

void ChassisController::pubError()
{
    if (reset_button_)
      event_msg_.ecode = 0;
    
    rclcpp::Time stamptmp = get_node()->now();

    event_msg_.header.stamp = stamptmp;
    event_msg_.header.frame_id = get_node()->get_name();
    if (event_msg_.ecode == 0) {
      event_msg_.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
      event_msg_.type = agv_msgs::msg::AGVEvent::INFO;
      event_msg_.action = agv_msgs::msg::AGVEvent::NONE;
    } else {
      event_msg_.eventtime = stamptmp.seconds();
      event_msg_.action = agv_msgs::msg::AGVEvent::STOP;
      event_msg_.type = agv_msgs::msg::AGVEvent::ALARM;
      event_msg_.ackreq = true;
    }

    if ((event_msg_.ecode == 0 && 
      (get_node()->now().seconds() - event_pub_time_ > 1.0)) || 
      event_msg_.ecode != 0) {
        event_pub_->publish(event_msg_);
        event_pub_time_ = get_node()->now().seconds();
    }
}

rcl_interfaces::msg::SetParametersResult 
ChassisController::parametersCallback(const std::vector<rclcpp::Parameter> & parameters) {
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;

  for (const auto & param : parameters) {
    if (param.get_name() == "offset_y_modification") {
      offset_y_ = param.as_double();
      DEBUG_OUT("Updated offset_y_modification to: %f", offset_y_);
    } else if (param.get_name() == "wheel_diameter_multiplier") {
      wheel_diameter_multiplier_ = param.as_double();
      DEBUG_OUT("Updated wheel_diameter_multiplier to: %f", wheel_diameter_multiplier_);
    } else if (param.get_name() == "pivot_offset_multiplier") {
      pivot_offset_multiplier_ = param.as_double();
      DEBUG_OUT("Updated pivot_offset_multiplier to: %f", pivot_offset_multiplier_);
    } else if (param.get_name() == "ring_mount_multiplier") {
      ring_mount_multiplier_ = param.as_double();
      DEBUG_OUT("Updated ring_mount_multiplier to: %f", ring_mount_multiplier_);
    } else if (param.get_name() == "left_wheel_radius_multiplier") {
      left_wheel_radius_multiplier_ = param.as_double();
      DEBUG_OUT("Updated left_wheel_radius_multiplier to: %f", left_wheel_radius_multiplier_);
    } else if (param.get_name() == "right_wheel_radius_multiplier") {
      right_wheel_radius_multiplier_ = param.as_double();
      DEBUG_OUT("Updated right_wheel_radius_multiplier to: %f", right_wheel_radius_multiplier_);
    }
  }
  update_dynamic_parameters();
  return result;
}

void ChassisController::update_dynamic_parameters()
{
  for (int i = 0; i < pivot_num_; ++i) {
    pivot_param_[i].driving_wheel_diameter = dynamic_pivot_params_.wheel_diameter[i] * wheel_diameter_multiplier_;
    pivot_param_[i].radius_ringmount = dynamic_pivot_params_.radius_ringmount[i] * ring_mount_multiplier_;
    pivot_param_[i].steer_offset = dynamic_pivot_params_.steer_offset[i] + steer_offset_addend[i];
    pivot_param_[i].offset_x = dynamic_pivot_params_.offset_x[i] * pivot_offset_multiplier_;
    pivot_param_[i].offset_y = dynamic_pivot_params_.offset_y[i] * pivot_offset_multiplier_;
    DEBUG_STREAM_OUT("Update driving_wheel_diameter[" << i << "]" << pivot_param_[i].driving_wheel_diameter);
    DEBUG_STREAM_OUT("Update radius_ringmount[" << i << "]" << pivot_param_[i].radius_ringmount);
    DEBUG_STREAM_OUT("Update offset_x[" << i << "]" << pivot_param_[i].offset_x);
    DEBUG_STREAM_OUT("Update offset_y[" << i << "]" << pivot_param_[i].offset_y);
  }
  pivot_param_[0].offset_y = dynamic_pivot_params_.offset_y[0] * pivot_offset_multiplier_ + offset_y_;
}

// bool ChassisController::reset()
// {
//   return true;
// }

// void ChassisController::halt()
// {
// }


}  // namespace chassis_controller

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  chassis_controller::ChassisController, controller_interface::ControllerInterface)
